#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

cv::Mat my_mean_filter(const cv::Mat& matPadded, int kwidth, int kheight) {
    CV_Assert(matPadded.type() == CV_8UC1);
    CV_Assert(kwidth % 2 == 1);
    CV_Assert(kheight % 2 == 1);
    int psWidth = kwidth / 2;
    int psHeight = kheight / 2;
    int rows = matPadded.rows - kheight + 1;
    int cols = matPadded.cols - kwidth + 1;
    int prod = kwidth * kheight;
    cv::Mat mat(rows, cols, CV_8UC1);
    for (int i = 0; i < rows; i++) {
        uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < cols; j++) {
            cv::Mat roi(matPadded, cv::Rect(j, i, kwidth, kheight));
            cv::Scalar sum = cv::sum(roi);
            p[j] = cv::saturate_cast<uchar>(sum[0] * 1. / prod);
        }
    }
    return mat;
}

cv::Mat get_2d_gaussian_kernel(int kwidth, int kheight, double sigmax, double sigmay) {
    if (sigmax == 0) {
        sigmax = 0.3 * ((kwidth * 1.0 - 1) * 0.5 - 1) + 0.8;
    }
    if (sigmay == 0) {
        sigmay = 0.3 * ((kheight * 1.0 - 1) * 0.5 - 1) + 0.8;
    }
    
    cv::Mat kernel(kheight, kwidth, CV_64FC1, cv::Scalar::all(0));
    double denom1 = 2. * (sigmax * sigmax);
    double denom2 = 2. * (sigmay * sigmay);
    int centerx = kwidth / 2;
    int centery = kheight / 2;
    for (int i = 0; i < kheight; i++) {
        double* p = kernel.ptr<double>(i);
        for (int j = 0; j < kwidth; j++) {
            double x = j * 1. - centerx;
            double y = i * 1. - centery;
            p[j] = cv::exp(-x*x/denom1-y*y/denom2);
        }
    }
    cv::Scalar sum;
    sum = cv::sum(kernel);
    kernel = kernel / sum;
    return kernel;
}

cv::Mat my_filter_2d_gaussian(const cv::Mat& matPadded, int kwidth, int kheight, double sigmax, double sigmay) {
    CV_Assert(matPadded.type() == CV_8UC1);
    CV_Assert(kwidth % 2 == 1);
    CV_Assert(kheight % 2 == 1);
    
    int psWidth = kwidth / 2;
    int psHeight = kheight / 2;
    int rows = matPadded.rows - kheight + 1;
    int cols = matPadded.cols - kwidth + 1;
    cv::Mat mat(rows, cols, CV_8UC1);
    cv::Mat kernel = get_2d_gaussian_kernel(kwidth, kheight, sigmax, sigmay);
    std::cout << "filter 2d gaussian kernel = \n" << cv::format(kernel, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    for (int i = 0; i < rows; i++) {
        uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < cols; j++) {
            cv::Mat roi(matPadded, cv::Rect(j, i, kwidth, kheight));
            cv::Mat roiDouble;
            roi.convertTo(roiDouble, CV_64FC1);
            cv::Mat dst;
            cv::multiply(roiDouble, kernel, dst);
            cv::Scalar sum = cv::sum(dst);
            p[j] = cv::saturate_cast<uchar>(sum[0] * 1.);
        }
    }
    return mat;
}

cv::Mat my_median_filter(const cv::Mat& matPadded, int ks) {
    CV_Assert(matPadded.type() == CV_8UC1);
    CV_Assert(ks % 2 == 1);
    int rows = matPadded.rows - ks + 1;
    int cols = matPadded.cols - ks + 1;
    cv::Mat mat(rows, cols, CV_8UC1);
    for (int i = 0; i < rows; i++) {
        uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < cols; j++) {
            cv::Mat roi(matPadded, cv::Rect(j, i, ks, ks));
            cv::Mat roiSorted;
            cv::sort(roi.clone().reshape(1, 1), roiSorted, cv::SORT_ASCENDING);
            p[j] = roiSorted.ptr<uchar>(0)[roiSorted.cols / 2];
        }
    }
    return mat;
}

// exp(-((i-k)^2+(j-l)^2 / (2*sigmaSpace^2)))
double get_space_factor(int i, int j, int k, int l, double sigmaSpace) {
    double sq1 = std::pow(i * 1.0 - k, 2);
    double sq2 = std::pow(j * 1.0 - l, 2);
    double denom = 2.0 * std::pow(sigmaSpace, 2);
    return std::exp(-(sq1 + sq2) / denom);
}

// exp(-0.5 * (fij-fkl)^2 / sigmaColor^2)
double get_color_factor(int fij, int fkl, double sigmaColor) {
    return std::exp(-std::pow(fij * 1.0 - fkl, 2) / (2.0 * std::pow(sigmaColor, 2)));
}

cv::Mat my_bilateral_filter(const cv::Mat& matPadded, int d, double sigmaColor, double sigmaSpace) {
    CV_Assert(matPadded.type() == CV_8UC1);
    CV_Assert(d > 0 && d % 2 == 1);
    
    int ps = d / 2;
    cv::Mat result(matPadded.rows - d + 1, matPadded.cols - d + 1, CV_8UC1);
    for (int i = 0; i < result.rows; i++) {
        for (int j = 0; j < result.cols; j++) {
            double numerator = 0;
            double denominator = 0;
            double fij = matPadded.ptr<uchar>(i + ps)[j + ps] * 1.0;
            for (int k = i-ps; k <= i+ps; k++) {
                for (int l = j-ps; l <= j+ps; l++) {
                    double fkl = matPadded.ptr<uchar>(k + ps)[l + ps] * 1.0;
                    double wijkl = get_space_factor(i, j, k, l, sigmaSpace) * get_color_factor(fij, fkl, sigmaColor);
                    numerator += fkl*wijkl;
                    denominator += wijkl;
                }
            }
            result.ptr<uchar>(i)[j] = cv::saturate_cast<uchar>(numerator / denominator);
        }
    }
    return result;
}

int main(int argc, char **argv) {
    cv::Mat mat(16, 16, CV_8UC1);
    cv::randu(mat, cv::Scalar::all(0), cv::Scalar::all(255));
    std::cout << "mat = \n" << cv::format(mat, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    //int kwidth = 5, kheight = 3;
    //cv::Mat matMean;
    //cv::blur(mat, matMean, cv::Size(kwidth, kheight), cv::Point(-1, -1), cv::BORDER_REFLECT101);
    //std::cout << "matMean = \n" << cv::format(matMean, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    //int psWidth = kwidth / 2;
    //int psHeight = kheight / 2;
    //cv::Mat matPadded;
    //cv::copyMakeBorder(mat, matPadded, psHeight, psHeight, psWidth, psWidth, cv::BORDER_REFLECT101);
    //cv::Mat matMyMean = my_mean_filter(matPadded, kwidth, kheight);
    //std::cout << "matMyMean = \n" << cv::format(matMyMean, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    //cv::Mat matMeanDiff = matMyMean - matMean;
    //std::cout << "matMeanDiff = \n" << cv::format(matMeanDiff, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    /*
    double sigmax = 2., sigmay = 1.;
    cv::Mat matFilter2DGaussian;
    cv::filter2D(mat, matFilter2DGaussian, CV_8UC1, get_2d_gaussian_kernel(kwidth, kheight, sigmax, sigmay), cv::Point(-1, -1), 0., cv::BORDER_REFLECT101);
    std::cout << "matFilter2DGaussian = \n" << cv::format(matFilter2DGaussian, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    cv::Mat matMyFilter2DGaussian = my_filter_2d_gaussian(matPadded, kwidth, kheight, sigmax, sigmay);
    std::cout << "matMyFilter2DGaussian = \n" << cv::format(matMyFilter2DGaussian, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    cv::Mat matFilter2DGaussianDiff = matMyFilter2DGaussian - matFilter2DGaussian;
    std::cout << "matFilter2DGaussianDiff = \n" << cv::format(matFilter2DGaussianDiff, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    cv::Mat matGaussianBlur;
    cv::GaussianBlur(mat, matGaussianBlur, cv::Size(kwidth, kheight), sigmax, sigmay, cv::BORDER_REFLECT101);
    std::cout << "matGaussianBlur = \n" << cv::format(matGaussianBlur, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    cv::Mat matSepFilter2DGaussianBlur;
    cv::sepFilter2D(mat, matSepFilter2DGaussianBlur, CV_8UC1, cv::getGaussianKernel(kwidth, sigmax), cv::getGaussianKernel(kheight, sigmay), cv::Point(-1, -1), 0., cv::BORDER_REFLECT101);
    std::cout << "matSepFilter2DGaussianBlur = \n" << cv::format(matSepFilter2DGaussianBlur, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    cv::Mat matGaussianBlurDiff = matSepFilter2DGaussianBlur - matGaussianBlur;
    std::cout << "matGaussianBlurDiff = \n" << cv::format(matGaussianBlurDiff, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    */
    
    /*
    int ksize = 9;
    cv::Mat matMedianBlur;
    cv::medianBlur(mat, matMedianBlur, ksize);
    std::cout << "matMedianBlur = \n" << cv::format(matMedianBlur, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    int ps = ksize / 2;
    cv::Mat matPadded;
    cv::copyMakeBorder(mat, matPadded, ps, ps, ps, ps, cv::BORDER_REFLECT);
    cv::Mat matMyMedianBlur = my_median_filter(matPadded, ksize);
    std::cout << "matMyMedianBlur = \n" << cv::format(matMyMedianBlur, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    cv::Mat matMedianBlurDiff = matMyMedianBlur - matMedianBlur;
    std::cout << "matMedianBlurDiff = \n" << cv::format(matMedianBlurDiff, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    */
    
    double sigmaColor = 20, sigmaSpace = 20;
    int d = 7;
    
    cv::Mat matBilateral;
    cv::bilateralFilter(mat, matBilateral, d, sigmaColor, sigmaSpace, cv::BORDER_REFLECT101);
    std::cout << "matBilateral = \n" << cv::format(matBilateral, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    int ps = d / 2;
    cv::Mat matPadded;
    cv::copyMakeBorder(mat, matPadded, ps, ps, ps, ps, cv::BORDER_REFLECT101);
    cv::Mat matMyBilateral = my_bilateral_filter(matPadded, d, sigmaColor, sigmaSpace);
    std::cout << "matMyBilateral = \n" << cv::format(matMyBilateral, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    cv::Mat matBilateralDiff = matMyBilateral - matBilateral;
    std::cout << "matBilateralDiff = \n" << cv::format(matBilateralDiff, cv::Formatter::FMT_PYTHON) << std::endl << std::endl;
    
    cv::Mat lena = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    if (lena.empty()) {
        std::cout << "Failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    sigmaColor = sigmaSpace = 50;
    cv::bilateralFilter(lena, matBilateral, d, sigmaColor, sigmaSpace, cv::BORDER_REFLECT101);
    cv::Mat lenaPadded;
    cv::copyMakeBorder(lena, lenaPadded, ps, ps, ps, ps, cv::BORDER_REFLECT101);
    matMyBilateral = my_bilateral_filter(lenaPadded, d, sigmaColor, sigmaSpace);
    cv::imshow("lena", lena);
    cv::imshow("bilateral", matBilateral);
    cv::imshow("myBilateral", matMyBilateral);
    
    cv::waitKey(0);
    
    return EXIT_SUCCESS;
}
